/*
MRL 2012
Written by Navid Nabavi
	   Alireza Hosseini
*/
#include<ros/ros.h>
#include<std_msgs/Bool.h>
#include<sensor_msgs/LaserScan.h>
#include <front_door/reconfigure.h>

class FrontDoorScanner
{
public:
    FrontDoorScanner(int argc,char* argv[]):configure_enabled(true),calb_ave(0)
    {

    }
    int run();
    bool reconfigureCB(front_door::reconfigure::Request &reg,
                     front_door::reconfigure::Response &res);
private:
    void laser_sub(const sensor_msgs::LaserScanConstPtr&);
    void door_pub(const bool);
    ros::Publisher door_situation_pub;
    ros::Subscriber laser_scan_sub;
    double calb[10];
    double calb_ave;
    bool configure_enabled;
    ros::ServiceServer reconfigure_srv;
    ros::NodeHandle nh;
};
/*##########################################################*/
int FrontDoorScanner::run()
{
    ros::Rate loop_rate(2);
    door_situation_pub=nh.advertise<std_msgs::Bool>("front_door/open",1);
    laser_scan_sub=nh.subscribe<sensor_msgs::LaserScan>("/scan",1,
        (boost::function < void(const sensor_msgs::LaserScanConstPtr&)>) boost::bind( &FrontDoorScanner::laser_sub, this, _1 ));
    reconfigure_srv = nh.advertiseService("reconfigure",&FrontDoorScanner::reconfigureCB,this);

    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
/*##########################################################*/
void FrontDoorScanner::door_pub(const bool flag)
{
    std_msgs::Bool msg;
    msg.data=flag;
    door_situation_pub.publish(msg);
}
/*##########################################################*/
void FrontDoorScanner::laser_sub(const sensor_msgs::LaserScanConstPtr &msg)
{
    double dist[10], dif[10],ave=0;
    if(configure_enabled)
    {
        calb_ave = 0;
        for(int i=0;i<10;i++)
        {
            calb[i]=msg->ranges.data()[i*4+340];
            calb_ave+=calb[i]/10;
        }
        configure_enabled=false;
        //std::cout<<"calb "<<calb_ave<<std::endl;
    }
    else
    {
        for(int i=0;i<10;i++)
        {
            dist[i]=msg->ranges.data()[i*4+340];
            ave+=dist[i]/10;
        }
        //std::cout<<"ave "<<ave<<std::endl;
        for(int i=0;i<10;i++)
        {
            dif[i]=dist[i]-calb[i];
        }
    }
    if(ave-calb_ave>/*2*/0.9 || calb_ave >= 0.7)
        door_pub(true);
    else
        door_pub(false);
}
/*##########################################################*/
bool FrontDoorScanner::reconfigureCB(front_door::reconfigure::Request &reg, front_door::reconfigure::Response &res)
{
    configure_enabled = true;
    ROS_INFO("reconfigure service request recieved.");
    return true;
}
/*##########################################################*/
int main(int argc,char* argv[])
{
    ros::init(argc,argv,"front_door");

    FrontDoorScanner fds(argc,argv);
    return fds.run();
}
